﻿// Accord Imaging Library
// The Accord.NET Framework
// http://accord-framework.net
//
// Copyright © César Souza, 2009-2014
// cesarsouza at gmail.com
//
//    This library is free software; you can redistribute it and/or
//    modify it under the terms of the GNU Lesser General Public
//    License as published by the Free Software Foundation; either
//    version 2.1 of the License, or (at your option) any later version.
//
//    This library is distributed in the hope that it will be useful,
//    but WITHOUT ANY WARRANTY; without even the implied warranty of
//    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
//    Lesser General Public License for more details.
//
//    You should have received a copy of the GNU Lesser General Public
//    License along with this library; if not, write to the Free Software
//    Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
//
// This work has been inspired by the original work of Peter Kovesi,
// shared under a permissive MIT license. Details are given below:
//
//   Copyright (c) 1995-2010 Peter Kovesi
//   Centre for Exploration Targeting
//   School of Earth and Environment
//   The University of Western Australia
//
//   Permission is hereby granted, free of charge, to any person obtaining a copy
//   of this software and associated documentation files (the "Software"), to deal
//   in the Software without restriction, including without limitation the rights 
//   to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies
//   of the Software, and to permit persons to whom the Software is furnished to do
//   so, subject to the following conditions:
//   
//   The above copyright notice and this permission notice shall be included in all
//   copies or substantial portions of the Software.
//   
//   The software is provided "as is", without warranty of any kind, express or
//   implied, including but not limited to the warranties of merchantability, 
//   fitness for a particular purpose and noninfringement. In no event shall the
//   authors or copyright holders be liable for any claim, damages or other liability,
//   whether in an action of contract, tort or otherwise, arising from, out of or in
//   connection with the software or the use or other dealings in the software.
//  
//
//   This Modification for Project AMEE @ HAW Hamburg
//   
//   Copyright (c) 2014 Bjoern Bettzueche
//   Department of Computer Science
//   Faculty Engineering and Computer Science
//   University of Applied Science Hamburg, Germany
//


using Accord.Math;
using Accord.Math.Decompositions;
using System;
using System.Collections.Generic;
using System.Threading.Tasks;

namespace AMEE.MashineLearning.Geometry
{
    /// <summary>
    /// Robust plane estimator with RANSAC.
    /// </summary>
    public class RansacPlane
    {
        /// <summary>
        /// 3. Minimum number of points to define a plane.
        /// </summary>
        public static readonly int MIN_PLANE_SAMPLES = 3;

        #region Fields

        /// <summary>the RANSAC estimator used (RANSAC&lt;Plane&gt;)</summary>
        private RANSAC<Plane> _ransac;
        private int[] _inliers;

        /// <summary>The set of (possibly noisy) points.</summary>
        private Point3[] _points;
        private double[] _distances;

        #endregion


        /// <summary>
        ///   Gets the RANSAC estimator used.
        /// </summary>
        public RANSAC<Plane> Ransac
        {
            get { return _ransac; }
        }

        /// <summary>
        /// Gets the final index set of inliers detected by RANSAC.<para>
        /// Result of last call of <c>RansacPlane.Estimate(Point3[])</c>.</para><para>
        /// Indexes corespond to given Point3 array, a sorted order is not guaranteed.</para>
        /// </summary>
        public int[] Inliers
        {
            get { return _inliers; }
        }


        /// <summary>
        ///   Creates a new RANSAC 3D plane estimator.
        /// </summary>
        /// <param name="threshold">Inlier threshold.</param>
        /// <param name="probability">Inlier probability. (normaly between 0.95 and 0.99)</param>
        public RansacPlane(double threshold, double probability)
        {
            // Create a new RANSAC with the selected threshold
            _ransac = new RANSAC<Plane>(MIN_PLANE_SAMPLES, threshold, probability);

            // Set RANSAC functions
            _ransac.Fitting = DefinePlaneFromPoints;
            _ransac.Distances = GetInliers;
            _ransac.Degenerate = IsDegenerated;
        }


        /// <summary>
        ///   Produces a robust estimation of the plane
        ///   passing through the given (noisy) points.
        /// </summary>
        /// <param name="points">A set of (possibly noisy) points.</param>
        /// <returns>The plane passing through the points. null, if #inliers &lt; 3</returns>
        public Plane Estimate(Point3[] points)
        {
            // Initial argument checks
            if (points.Length < MIN_PLANE_SAMPLES)
                throw new ArgumentException("At least three points are required to fit a plane");

            _distances = new double[points.Length];
            _points = points;

            // Compute RANSAC and find the inlier points
            Plane bestModel = _ransac.Compute(points.Length, out _inliers);

            if (_inliers.Length == 0)
                return null;

            // Compute the final plane
            // TODO: wieso?? Ransac.Compute liefert schon ein best Model!
            // Plane plane = fitting(points.Submatrix(_inliers));
            //
            // return plane;
            return bestModel;
        }



        /// <summary>
        /// Defines the plane (model) from three points.
        /// </summary>
        /// <param name="x">The three point's indexes</param>
        /// <returns>A plane model</returns>
        /// <exception cref="IndexOutOfRangeException">if less then 3 x'es</exception>
        /// <exception cref="NullReferenceException">x is null</exception>
        private Plane DefinePlaneFromPoints(int[] x)
        {
            Point3 p1 = _points[x[0]];
            Point3 p2 = _points[x[1]];
            Point3 p3 = _points[x[2]];

            return Plane.FromPoints(p1, p2, p3);
        }

        /// <summary>
        /// Gets the inliers.<para>
        /// Delegate function for <c>Ransac.Distances</c>.
        /// </para>
        /// </summary>
        /// <param name="p">The plane model.</param>
        /// <param name="threshold">The threshold.</param>
        /// <returns>indexes of <em>good</em> points.</returns>
        /// <exception cref="NullReferenceException"></exception>
        /// <remarks>
        /// First call <c>Estimate(Point3[])</c> and pass on the point cloud.
        /// This method returns the indices of that points in the given <c>Point3</c> array,
        /// whose distance to the specified <c>Plane</c> is within the specified <c>threshold</c>.
        /// A sorted index order is not guaranteed!
        /// </remarks>
        private int[] GetInliers(Plane p, double threshold) {
            List<int> indexes = new List<int>();
            var syncRoot = new Object();

            // Note: the result is no longer sorted!
            Parallel.For<List<int>>(0, _points.Length,
                () => new List<int>(), // thread local sub-list init
                (idx, loopState, subList) => {
                    double dist = p.DistanceToPoint(_points[idx]);
                    if (dist < threshold) {
                        subList.Add(idx);
                    }
                    return subList;
                },
                subList => { // merge thread local sub-lists
                    lock (syncRoot) {
                        indexes.AddRange(subList);
                    }
                }
            );
            return indexes.ToArray();
        }


        /// <summary>
        /// Determines whether the plane (specified by point indices) is degenerated.
        /// </summary>
        /// <param name="indices">The indices, at least three</param>
        /// <returns>True, if the first three Points lie on the same line</returns>
        /// <exception cref="IndexOutOfRangeException">if less then 3 indices</exception>
        /// <exception cref="NullReferenceException"><c>indices</c> is null</exception>
        bool IsDegenerated(int[] indices) {
            Point3 p1 = _points[indices[0]];
            Point3 p2 = _points[indices[1]];
            Point3 p3 = _points[indices[2]];

            return Point3.Collinear(p1, p2, p3);
        }


        /// <summary>
        /// Fittings the specified points.
        /// </summary>
        /// <param name="points">The points.</param>
        /// <returns></returns>
        static Plane fitting(Point3[] points)
        {
            // Set up constraint equations of the form  AB = 0,
            // where B is a column vector of the plane coefficients
            // in the form   b(1)*X + b(2)*Y +b(3)*Z + b(4) = 0.
            //
            // A = [XYZ' ones(npts,1)]; % Build constraint matrix
            if (points.Length < MIN_PLANE_SAMPLES)
                return null;

            if (points.Length == MIN_PLANE_SAMPLES)
                return Plane.FromPoints(points[0], points[1], points[2]);

            float[,] A = new float[points.Length, 4];
            for (int i = 0; i < points.Length; i++)
            {
                A[i, 0] = points[i].X;
                A[i, 1] = points[i].Y;
                A[i, 2] = points[i].Z;
                A[i, 3] = -1;
            }

            SingularValueDecompositionF svd = new SingularValueDecompositionF(A,
                computeLeftSingularVectors: false, computeRightSingularVectors: true,
                autoTranspose: true, inPlace: true);

            float[,] v = svd.RightSingularVectors;

            float a = v[0, 3];
            float b = v[1, 3];
            float c = v[2, 3];
            float d = v[3, 3];

            float norm = (float)Math.Sqrt(a * a + b * b + c * c);

            a /= norm;
            b /= norm;
            c /= norm;
            d /= norm;

            return new Plane(a, b, c, -d);
        }

    }
}
